//
//  SQT.cpp
//  TrackingSim
//
//  Created by Garrett Manka on 12/18/11.
//  Copyright (c) 2011. All rights reserved.
//

#include <math/SQT.h>

SQT::SQT()
{
}

SQT::SQT( double s, const Quaternion& q, const Sim::Vector& t):m_s(s),m_q(q),m_t(t)
{
}

Matrix4x4 SQT::getMatrix4x4() const
{
    double x2 = m_q.x() * m_q.x();
    double y2 = m_q.y() * m_q.y();
    double z2 = m_q.z() * m_q.z();
    double xy = m_q.x() * m_q.y();
    double xz = m_q.x() * m_q.z();
    double yz = m_q.y() * m_q.z();
    double wx = m_q.w() * m_q.x();
    double wy = m_q.w() * m_q.y();
    double wz = m_q.w() * m_q.z();
    
    return Matrix4x4(1.0 - 2.0*(y2+z2),       2.0*(xy-wz),       2.0*(xz+wy), 0.0,
                           2.0*(xy+wz), 1.0 - 2.0*(x2+z2),       2.0*(yz-wx), 0.0,
                           2.0*(xz-wy),       2.0*(yz+wx), 1.0 - 2.0*(x2+y2), 0.0,
                               m_t.x(),           m_t.y(),           m_t.z(), 1.0);
    
}